// #include<ros/ros.h>
// #include <nav_msgs/Odometry.h>

// void ShowLocation(const nav_msgs::Odometry::ConstPtr& msg) {
//     // 提取x和y方向的位置信息
//     double x = msg->pose.pose.position.x;
//     double y = msg->pose.pose.position.y;



//     // 输出处理后的位置信息
//     ROS_INFO("Filtered Position: x = %f, y = %f", x, y);
// }

// int main(int argc,char *argv[])
// {
//     ros::init(argc,argv,"Location");
//     ros::NodeHandle nh;
    
//     ros::Subscriber gps_sub = nh.subscribe<nav_msgs::Odometry>("/J001/odometry/filtered",10,ShowLocation);
    
//     while(ros::ok())
//     {
//         ros::spinOnce();
//     }
//     return 0;
    
// }

#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <deque>

// 定义一个简单的移动平均滤波器
class MovingAverageFilter {
private:
    std::deque<double> x_values, y_values;
    int window_size;

public:
    MovingAverageFilter(int size) : window_size(size) {}

    void addValue(double x, double y) {
        x_values.push_back(x);
        y_values.push_back(y);
        if (x_values.size() > window_size) {
            x_values.pop_front();
            y_values.pop_front();
        }
    }

    std::pair<double, double> getFilteredValue() const {
        if (x_values.empty()) {
            return {0, 0};
        }
        double sum_x = 0, sum_y = 0;
        for (double val : x_values) sum_x += val;
        for (double val : y_values) sum_y += val;
        return {sum_x / x_values.size(), sum_y / y_values.size()};
    }
};

void ShowLocation(const nav_msgs::Odometry::ConstPtr& msg, MovingAverageFilter& filter) {
    // 提取x和y方向的位置信息
    double x = msg->pose.pose.position.x;
    double y = msg->pose.pose.position.y;

    // 添加到滤波器
    filter.addValue(x, y);

    // 获取过滤后的位置信息
    auto [filtered_x, filtered_y] = filter.getFilteredValue();

    // 输出处理后的位置信息
    ROS_INFO("Filtered Position: x = %f, y = %f", filtered_x, filtered_y);
}

int main(int argc, char *argv[]) {
    ros::init(argc, argv, "Location");
    ros::NodeHandle nh;

    // 初始化滤波器，设置窗口大小
    MovingAverageFilter filter(5); // 这里设置为5，您可以根据需要调整

    ros::Subscriber gps_sub = nh.subscribe<nav_msgs::Odometry>("/J001/odometry/filtered", 10, boost::bind(ShowLocation, _1, boost::ref(filter)));

    while (ros::ok()) {
        ros::spinOnce();
    }
    return 0;
}